// #include <Kinectv3.h>
// #include <opencv2/core.hpp>
#include "LdPcl.h"
int main()
{
    // std::string pcd = "/home/milo/Documents/visual/2Champion/Lidars/Livox/Mars/catkin_ws/src/livox_camera_calib/data/.pcd";
    // Pcloud show(new Cloud());
    // pcl::io::loadPCDFile(pcd, *show);
    // pcl::visualization::CloudViewer viewer2("示例");
    // viewer2.showCloud(show);
    // while (!viewer2.wasStopped())
    // {
    // }
    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    LdsLidar &PcdRecord = LdsLidar::GetInstance();
    cv::namedWindow("Display", cv::WINDOW_FREERATIO);
    int resultLidar = PcdRecord.InitLdsLidar(BroadCode, 480000, 1, 0);
    if (!resultLidar)
    {
        std::cout << "Init Lidar success!" << std::endl;
    }
    else
    {
        std::cout << "Failed!" << std::endl;
    }
    // hitcrt::Kinectv3 cam("auto", false);
    char key;
    pcl::visualization::CloudViewer viewer("viewer1");
    while (key != 'q')
    {
        //     std::string resultCam = cam.grab();
        //     if (resultCam == "failed")
        //     {
        //         std::cout << "抓图出现未知错误" << std::endl;
        //         continue;
        //     }
        //     else if (resultCam == "init")
        //     {
        //         std::cout << "初始化未成功" << std::endl;
        //         continue;
        //     }
        //     auto colorImg = cam.getColorImage();
        //     cv::Mat Img;
        //     if (colorImg == NULL && colorImg->empty())
        //     {
        //         continue;
        //     }
        //     cv::cvtColor(*colorImg, Img, cv::COLOR_BGRA2BGR);

        //     cv::imshow("Display", Img);
        cv::Mat DepthImg = cv::Mat::zeros(1024, 1280, CV_16UC1);
        // PcloudI cloud(new CloudI());
        bool ifGet = false;
        Pcloud cloud(new Cloud());
        //获取雷达点云
        while (!ifGet)
        {
            // PcdRecord.getCloudImg(cloud, DepthImg);
            PcdRecord.getcloud(cloud);
            if (cloud != nullptr)
                ifGet = true;
        }

        char key = cv::waitKey(1);
        if (key == 's')
        {
            viewer.showCloud(cloud);
            while (!viewer.wasStopped())
            {
            }
            std::string save_pcd_path = "/home/milo/Documents/visual/2Champion/Lidars/Livox/Mars/catkin_ws/src/livox_camera_calib/data/1.pcd";
            //
            // std::stringstream sstream, sstream1, sstream2;
            // std::string pathcolor = "/home/milo/Documents/visual/2Champion/Lidars/Livox/Mars/catkin_ws/src/livox_camera_calib/data/8.png";
            // sstream >> pathcolor;
            // if (!colorImg->empty())
            // {
            // cv::Mat temp = colorImg->clone();
            // cv::imwrite(pathcolor, Img);
            cloud->height = 1;
            cloud->width = cloud->size();
            pcl::io::savePCDFile(save_pcd_path, *cloud);
            break;
            // }
        }
        else if (key == 32)
        {
            while (cv::waitKey(0) != 32)
                cv::waitKey(0);
        }
    }

    // cam.init();
    // cam.record("/home/milo/ball.mkv");
}